#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Classifier.h"
#include <iostream>
#include <fstream>
#include <drawstuff/drawstuff.h>
#include <string>
#include "main.h"
#include <utility>
#include <stack>

#define GOALX 0.20
#define GOALY 0.00
#define d 0.01

using namespace std;

struct step { //the instructions necessary for a single robot step
  public:
     int foot;
     float x;
     float y;

     step(Node* targetNode){
        foot = targetNode->moving_foot;
        x = targetNode->foot_pos[foot].n[0];
        y = targetNode->foot_pos[foot].n[1];
     }   
};

class smallest_f // comparison function for priority queue
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
    return (lhs->f > rhs->f);
  }
};

class smallest_node // comparison function for set
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
     for(int i = 0; i < 4; i++){
        for(int j = 0; j < 3; j++){
           float left = lhs->foot_pos[i].n[j];
           float right = rhs->foot_pos[i].n[j];
           if(fabs(left - right) > d && left < right){ 
              return true;
           }
        }
     }
     return false;
  }
};
 
  float EuclidDist(float deltaX, float deltaY) { //Calculates the Euclidian Distance of an ordered pair
     return sqrt(deltaX*deltaX + deltaY*deltaY);
  }

  bool GoalTest(Node * testNode) { //determines whether a state/node is above the goal position
     return EuclidDist(testNode->center[0] - GOALX,testNode->center[1]-GOALY) < d;
  }

  
  void FindPath(stack<step>* path, Node* currentNode){ //Recursively backtracks the A* search path
      if(currentNode->parent != NULL){
         path->push(step(currentNode));
         FindPath(path, currentNode->parent);
      }
  }

  void SavePathToFile(stack<step> * path){
     ofstream pathFile("test.path");
     if(!pathFile){
        printf("Error opening the file!\n");
     }
     while(!path->empty()){
        step nextStep = path->top();
        path->pop();
        pathFile << nextStep.foot << " " << nextStep.x << " " << nextStep.y << endl;
     }
  }

  void PrintFootPositions(Node* node){
     printf("Node address: %d\n", node);
     printf("f: %f\n", node->f);
     printf("center: (%f,%f)\n\n", node->center[0], node->center[1]);
     bduVec3f* globalFeet = node->foot_pos;
     printf("feet : \n%f, %f, %f \n%f, %f, %f\n%f, %f, %f\n%f, %f, %f\n", globalFeet[0].n[0],globalFeet[0].n[1],globalFeet[0].n[2],globalFeet[1].n[0], globalFeet[1].n[1],globalFeet[1].n[2],globalFeet[2].n[0],globalFeet[2].n[1],globalFeet[2].n[2],globalFeet[3].n[0],globalFeet[3].n[1],globalFeet[3].n[2]);
  }

  float* FindNodeCenter(Node* node){
     float* center = new float[2];
     center[0] = 0;
     center[1] = 0;
     for(int i = 0; i < 4; i++){
        center[0] += node->foot_pos[i].n[0];
        center[1] += node->foot_pos[i].n[1];
     }
     center[0] /= 4;
     center[1] /= 4;
     return center;
  }

  void PrepNodeForDeletion(Node* node){
     delete node->center;
     delete node->foot_pos;
  }

  Node* GenerateChildNode(SDSimulator* simulator, /*Classifier classifier,*/ Node* current,int i,int j){
     Node* child = new Node();
     child->foot_pos = new bduVec3f[4];
     for(int k = 0; k < 4; k++){
        if(current->moving_foot != k){
           child->foot_pos[k] = current->foot_pos[k];
        }else{
           float x = current->foot_pos[k].n[0] + i*d*2;
           float y = current->foot_pos[k].n[1] + j*d*2;
           child->foot_pos[k].n[0] = x;
           child->foot_pos[k].n[1] = y;
           child->foot_pos[k].n[2] = simulator->getPointHeight(x,y);
        }
     }

     child->moving_foot = (current->moving_foot + 1)%4;
     child->center = FindNodeCenter(child);
     child->h = 4*sqrt(pow(child->center[0]-GOALX,2) + pow(child->center[1]-GOALY,2))/d;
     child->g = current->g + 1;
     child->f = child->g + child->h;
     child->parent = current;
     return child;
  }
  
  bool ValidStep(Node* node, SDSimulator* simulator, SDSlowWalk* walkController, int* number){
   //  return true;

     if(node->parent == NULL){
        printf("validating start node.\n");
        (*number)++;
        return true;
     }
 
     PrintFootPositions(node->parent);


     //printf(">>>Accessing Parent state...\n");
     simulator->setState(node->parent->state);
     //printf("------Parent state address: %d\n", node->state);
     step stepParam = step(node);
     walkController->setStepPosition(stepParam.foot, stepParam.x, stepParam.y);
     bduVec3f globalFeet[4];
     simulator->getGlobalFootPositions(globalFeet);
     float x = stepParam.x;//node->foot_pos[stepParam.foot].n[0];
     float y = stepParam.y;//node->foot_pos[stepParam.foot].n[1];
     int result;
     float zoff = 0;
	for(int z=0; z<4; z++){ //this loop checks if the dog has flipped
		if((globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1])) > 0)
		   zoff += (globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
		else
		   zoff += -1*(globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
  	}

	if( zoff > 0.2 ){
	   result = 1;
	   printf("omg it flipped!\n");
	}
        else if((x+ .01 > globalFeet[node->moving_foot].n[0]) && (x- .01 < globalFeet[node->moving_foot].n[0]) && (y+.01 > globalFeet[node->moving_foot].n[1]) && (y-.01 < globalFeet[node->moving_foot].n[1])){
            result = 0;
        } 
        else{
            result = 1;
	}
    if(result == 0){
       SDSimulatorState* state = new SDSimulatorState();
       simulator->getState(state);
       node->state = state;
       (*number)++;
       return true;
    }
    return false;//*/
  }
   
int main(int argc, char** argv){
  // Initialization
  SDSimulator *simulator;
  SDLittleDog littleDog;
  SDSlowWalk walkController;

  littleDog.setSimulated(true);
  littleDog.setController(&walkController);
  walkController.setLittleDog(&littleDog);
  
  littleDog.initializeSimulator();
  simulator = littleDog.getSimulator();

  littleDog.setSimUI(false);
  
// A* algorithm

  // Create start node
  bduVec3f globalFeet[4];
  simulator->getGlobalFootPositions(globalFeet);
  Node nstart;
  nstart.foot_pos = globalFeet;
  nstart.moving_foot = 0;
  nstart.g = 0;
  //float center[2];
  //center = 
//{(globalFeet[0].n[0]+globalFeet[1].n[0]+globalFeet[2].n[0]+globalFeet[3].n[0])/4 , (globalFeet[0].n[1]+globalFeet[1].n[1]+globalFeet[2].n[1]+globalFeet[3].n[1])/4};
  nstart.center = FindNodeCenter(&nstart);
  nstart.h = 4*sqrt(pow(nstart.center[0]-GOALX,2) + pow(nstart.center[1]-GOALY,2))/d;
  nstart.f = nstart.g + nstart.h;
  nstart.parent = NULL;
  SDSimulatorState startState;
  simulator->getState(&startState);
  nstart.state = &startState;
  
  //Initialize queue, set, and other variables
  priority_queue<Node*, vector<Node*>, smallest_f> Q;
  stack<step> path;
  Q.push(&nstart);
  PrintFootPositions(&nstart);
  multiset<Node*, smallest_node> expanded;
  multiset<Node*, smallest_node>::iterator it;
  //Classifier classifier = new Classifier(0); //NEEDS TO BE DONE CORRECTLY
  


/*
     Node node2;
     node2.foot_pos = new bduVec3f[4]; // pointer on bduVec3f : will be size 4

     node2.foot_pos[0].n[0] = 3;
     node2.foot_pos[0].n[1] = 1;
     node2.foot_pos[0].n[2] = 0;
     
     node2.foot_pos[1].n[0] = 3;
     node2.foot_pos[1].n[1] = -1;
     node2.foot_pos[1].n[2] = 0;

     node2.foot_pos[2].n[0] = 1;
     node2.foot_pos[2].n[1] = 1;
     node2.foot_pos[2].n[2] = 0;

     node2.foot_pos[3].n[0] = 1;
     node2.foot_pos[3].n[1] = -1;
     node2.foot_pos[3].n[2] = 0;

     node2.moving_foot = 1; //number of the current moving foot
     node2.center = FindNodeCenter(&node2); //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
     node2.h = 0; // heuristic
     node2.g = 1; // cost
     node2.f = 1; // f=g+h is used to compare nodes
     node2.parent = &nstart;
 
     PrintFootPositions(&node2);
     Q.push(node2);printf("pushed node2.\n");
 */

  int yoyo = 0;
  for(yoyo = 1; yoyo < 20; yoyo++)
  printf("yoyo = %d (should be 20)\n", yoyo + 1);

  //actual A* algorithm loop structure
  int numberValidated = 0;
  while(!Q.empty()){
     Node* current = Q.top();
     Q.pop();
     it=expanded.find(current);
     if(it==expanded.end()){
        if(ValidStep(current, simulator, &walkController, &numberValidated)){
           if(GoalTest(current)){
              FindPath(&path, current);
              PrintFootPositions(current);
              break;
           }
           expanded.insert(current);
           for(int i = -2; i < 3; i++){
              for(int j = -2; j < 3; j++){
                 if(i != 0 && j != 0){
                    Node* child = GenerateChildNode(simulator, /*classifier,*/ current, i, j);
                    if(child != NULL){
                       Q.push(child);
                    }
                 }
              }  
           }     
        }
     }
  } 
  
  printf("Num validated = %d, Q size = %d, expanded size = %d\n", numberValidated, Q.size(), expanded.size());

  //Save the path (stored in a struct) to a path file
  if(path.empty()){
     printf("Path not found\n");
  }else{
     SavePathToFile(&path);
  }
  
  //Delete all nodes and classifier (so we aren't orphaning memory)
  
  //delete classifier;
  for(multiset<Node*, smallest_node>::iterator it = expanded.begin();it != expanded.end() && *it != &nstart; it++){
     delete *it;
  }
  expanded.clear();
  if(!Q.empty()){
     for(Node* next = Q.top(); !Q.empty(); next = Q.top()){
        Q.pop();
        delete next;
     }
  }
  
  //Exit program
  
  return 0; 
}







